//
// GPSMapEdit
// (c) Konstantin Galichsky (kg@geopainting.com), 2005
//
// Fast sin & cos.
//

# include "StdAfx.h"
# include "fast_sqrt.h"
# include "fast_floor.h"
# include "Globals.h"

const size_t c_cSinSize = 90;
const float g_fSin [c_cSinSize + 1] = {
	0,
	0.017452406437283512819418978516316f,
	0.034899496702500971645995181625333f,
	0.052335956242943832722118629609078f,
	0.069756473744125300775958835194143f,
	0.087155742747658173558064270837474f,
	0.1045284632676534713998341548025f,
	0.12186934340514748111289391923153f,
	0.13917310096006544411249666330111f,
	0.15643446504023086901010531946717f,
	0.17364817766693034885171662676931f,
	0.19080899537654481240514048795839f,
	0.20791169081775933710174228440513f,
	0.2249510543438649980511072083428f,
	0.24192189559966772256044237410035f,
	0.25881904510252076234889883762405f,
	0.2756373558169991856499715746113f,
	0.29237170472273672809746869537714f,
	0.30901699437494742410229341718282f,
	0.32556815445715666871400893579472f,
	0.34202014332566873304409961468226f,
	0.35836794954530027348413778941347f,
	0.3746065934159120354149637745012f,
	0.39073112848927375506208458888909f,
	0.4067366430758002077539859903415f,
	0.42261826174069943618697848964773f,
	0.43837114678907741745273454065827f,
	0.45399049973954679156040836635787f,
	0.46947156278589077595946228822784f,
	0.48480962024633702907537962241578f,
	0.5f,
	0.51503807491005421008163193639814f,
	0.52991926423320495404678115181609f,
	0.54463903501502708222408369208157f,
	0.55919290347074683016042813998599f,
	0.57357643635104609610803191282616f,
	0.58778525229247312916870595463907f,
	0.60181502315204827991797700044149f,
	0.61566147532565827966881109284366f,
	0.62932039104983745270590245827997f,
	0.64278760968653932632264340990726f,
	0.65605902899050728478249596402342f,
	0.66913060635885821382627333068678f,
	0.68199836006249850044222578471113f,
	0.69465837045899728665640629942269f,
	0.70710678118654752440084436210485f,
	0.71933980033865113935605467445671f,
	0.73135370161917048328754360827562f,
	0.74314482547739423501469704897426f,
	0.75470958022277199794298421956102f,
	0.76604444311897803520239265055542f,
	0.7771459614569708799799377436724f,
	0.78801075360672195669397778783585f,
	0.79863551004729284628400080406894f,
	0.80901699437494742410229341718282f,
	0.81915204428899178968448838591684f,
	0.82903757255504169200633684150164f,
	0.83867056794542402963759094180455f,
	0.84804809615642597038617617869039f,
	0.85716730070211228746521798014476f,
	0.86602540378443864676372317075294f,
	0.87461970713939580028463695866108f,
	0.88294759285892694203217136031572f,
	0.89100652418836786235970957141363f,
	0.89879404629916699278229567669579f,
	0.90630778703664996324255265675432f,
	0.91354545764260089550212757198532f,
	0.92050485345244032739689472330046f,
	0.92718385456678740080647445113696f,
	0.93358042649720174899004306313957f,
	0.93969262078590838405410927732473f,
	0.9455185755993168103481247075194f,
	0.95105651629515357211643933337938f,
	0.95630475596303548133865081661842f,
	0.96126169593831886191649704855706f,
	0.9659258262890682867497431997289f,
	0.97029572627599647230637787403399f,
	0.97437006478523522853969448008827f,
	0.9781476007338056379285667478696f,
	0.98162718344766395349650489981814f,
	0.98480775301220805936674302458952f,
	0.98768834059513772619004024769344f,
	0.99026806874157031508377486734485f,
	0.99254615164132203498006158933058f,
	0.99452189536827333692269194498057f,
	0.99619469809174553229501040247389f,
	0.99756405025982424761316268064426f,
	0.99862953475457387378449205843944f,
	0.99939082701909573000624344004393f,
	0.99984769515639123915701155881391f,
	1
};

float fast_sin (float _f) {
//	return ::sin (_f);

	assert (-2*c_PI <= _f && _f <= 2*c_PI);
	if (! (-2*c_PI <= _f && _f <= 2*c_PI))
		return ::sin (_f);

	float a = _f/(.5f*c_PI);
	if (_f < 0)
		a = -a;
	int nSign = 1;
	if (a > 2) {
		a = 4 - a;
		nSign = -1;
	}
	if (a > 1)
		a = 2 - a;
	const size_t nSin = fast_floor (a*c_cSinSize + .5f);
	assert (nSin <= c_cSinSize);
	float ret = nSign*g_fSin [nSin];
	if (_f < 0)
		ret = -ret;

# ifdef _DEBUG
	float r2 = ::sin (_f);
	assert (::fabs (r2 - ret) < 0.01);
# endif

	return ret;
}

float fast_cos (float _f) {
//	return ::cos (_f);

	assert (-2*c_PI <= _f && _f <= 2*c_PI);
	if (! (-2*c_PI <= _f && _f <= 2*c_PI))
		return ::cos (_f);

	float a = _f/(.5f*c_PI);
	int nSign = 1;
	if (_f < 0)
		a = -a;
	if (a > 2)
		a = 4 - a;
	if (a > 1) {
		a = 2 - a;
		nSign = -1;
	}
	const size_t nSin = fast_floor ((1 - a)*c_cSinSize + .5f);
	assert (nSin <= c_cSinSize);
	float ret = nSign*g_fSin [nSin];

# ifdef _DEBUG
	float r2 = ::cos (_f);
	assert (::fabs (r2 - ret) < 0.01);
# endif

	return ret;
}

const size_t c_cAtanSize = 100;
const float g_fAtan [c_cAtanSize + 1] = {
	0,
	0.0099996666866652382063401162092795f,
	0.019997333973150533060753196901596f,
	0.029991004856877899676512459459942f,
	0.039978687123290041413577849629375f,
	0.049958395721942761410006287034845f,
	0.059928155121207884431815313861521f,
	0.069886001634642499294980487984493f,
	0.079829985712237315892954191567332f,
	0.089758174189950523149787239319397f,
	0.099668652491162027378446119878021f,
	0.10955952677394434486699241285981f,
	0.11942892601833845180672111641376f,
	0.12927500404814305471599938326971f,
	0.13909594148207132429654283387275f,
	0.14888994760949725058653039165587f,
	0.15865526218640140386370801978126f,
	0.16839015714752989727502090172968f,
	0.17809293823119754966792029959123f,
	0.18776194651359341520351091246245f,
	0.19739555984988075837004976519479f,
	0.20699219421982102489585928037958f,
	0.21655030497608927648197931498645f,
	0.2260683879938839058423534599328f,
	0.23554498072086334143497512343376f,
	0.24497866312686415417208248121128f,
	0.25436805855326593142885712332461f,
	0.26371183446226612016114786232071f,
	0.27300870308671060294905982523562f,
	0.28225742198149112126814653318423f,
	0.29145679447786709199560462143289f,
	0.3006056700423954042258423224989f,
	0.30970294454245619991738081039242f,
	0.31874756042064443711750859451907f,
	0.32773850678055544599683540615889f,
	0.33667481938672718139669863134177f,
	0.34555558058171213685527266971645f,
	0.35437991912343780983072635114399f,
	0.36314700994617628972394501939422f,
	0.37185607384858125749596487910426f,
	0.38050637711236488630358791681043f,
	0.38909723105527841924428129061132f,
	0.39762799152212931399954386888093f,
	0.40609805831761564783063575560739f,
	0.41450687458478593834143065673049f,
	0.42285392613294071296648279098114f,
	0.43113874071878218339273183639798f,
	0.43936088728459143742224135336384f,
	0.44751997515716987972164272693692f,
	0.4556156532112244921374614168227f,
	0.46364760900080611621425623146121f,
	0.47161556786232766012001382320594f,
	0.47951929199259616542309704129544f,
	0.48735857950519028311540551635107f,
	0.49513326346840412184872016600183f,
	0.50284321092786082733088202924528f,
	0.51048832191677576996647999146307f,
	0.51806852845672094899638472688591f,
	0.52558379355161020277163196763958f,
	0.53303411017749002604051892171453f,
	0.5404195002705841554435783646086f,
	0.54774001371590245052057233188898f,
	0.55499572733858676241686977644094f,
	0.56218674390002917484735672680714f,
	0.56931319110066188631299209633831f,
	0.57637522059118368022757047839377f,
	0.58337300699385593947030011867094f,
	0.59030674693537198238982903713835f,
	0.59717665809267754844430657194869f,
	0.60398297825299790737726733146002f,
	0.61072596438920861654375887649024f,
	0.61740589175157266651971394484851f,
	0.62402305297675684758979054708821f,
	0.63057775721493480666483920791007f,
	0.63707032927568357172415033415913f,
	0.64350110879328438680280922871732f,
	0.64987044941194757748415541022072f,
	0.65617871799139487538392025922877f,
	0.66242629383315116176872226712701f,
	0.66861356792782091069195819371662f,
	0.67474094222355266305652097360981f,
	0.68080882891582756648868049233182f,
	0.6868176497586452755351792504699f,
	0.69276783539712221066245282985001f,
	0.69865982472146319780475095283515f,
	0.704494064242217716657480340782f,
	0.71027100748668623033434311380219f,
	0.71599111441630019894329649130695f,
	0.7216548508647612370694394803436f,
	0.72726268799669029804993899218082f,
	0.73281510178650659164079207273428f,
	0.73831257251722800021208349902745f,
	0.74375558429885988575548278111912f,
	0.74914462460601721032422782543075f,
	0.75448018383440566231125881895763f,
	0.75976275487577082892296119539998f,
	0.76499283271091022317293250580695f,
	0.77017091402033100725861937873632f,
	0.77529749681212640304368744720001f,
	0.78037308006663589889787151727255f,
	0.78539816339744830961566084581988f,
};

float fast_atan2 (long _dy, long _dx) {
//return ::atan2 (_dy, _dx);
	if (_dy == 0)
		return 0;
	if (_dx == 0)
		return _dy > 0 ? c_PI/2 : -c_PI/2;
	if (_dx < 0) {
		_dx = -_dx;
		_dy = -_dy;
	}
	const float fabs_dy = _dy < 0 ? -_dy : _dy;
	float ret;
	if (fabs_dy < _dx) {
		const float r = fabs_dy/_dx;
		ret = g_fAtan [fast_floor (r*c_cAtanSize + .5f)];
	} else {
		const float r = _dx/fabs_dy;
		ret = c_PI/2 - g_fAtan [fast_floor (r*c_cAtanSize + .5f)];
	}
	ret = _dy < 0 ? -ret : ret; 

# ifdef _DEBUG
	float r2 = ::atan2 (float (_dy), float (_dx));
	assert (::fabs (r2 - ret) < 0.005);
# endif

	return ret;
}
